#include "app_tentacle.h"
#include "app.h"

// 多节触角可视化示例

// 打印触角形状
void Tentacle_Print_Shape(void)
{
    DEBUG("=== Tentacle Shape ===\n");
    DEBUG("End Position: (%.1f, %.1f, %.1f)\n", 
          g_tentacle_ctrl.current.x, 
          g_tentacle_ctrl.current.y, 
          g_tentacle_ctrl.current.z);
    
    for (int i = 0; i < TENTACLE_SEGMENTS; i++) {
        DEBUG("Segment %d: pos(%.1f, %.1f, %.1f), radius=%.1f, angles(%.2f, %.2f)\n",
              i+1, 
              g_tentacle_ctrl.current.segments[i].x,
              g_tentacle_ctrl.current.segments[i].y,
              g_tentacle_ctrl.current.segments[i].z,
              g_tentacle_ctrl.current.segments[i].radius,
              g_tentacle_ctrl.current.segments[i].angle_x,
              g_tentacle_ctrl.current.segments[i].angle_y);
    }
    DEBUG("=====================\n");
}

// 计算触角总长度
float Tentacle_Calculate_Total_Length(void)
{
    float total_length = 0;
    for (int i = 0; i < TENTACLE_SEGMENTS; i++) {
        if (i == 0) {
            total_length += sqrt(g_tentacle_ctrl.current.segments[i].x * g_tentacle_ctrl.current.segments[i].x +
                               g_tentacle_ctrl.current.segments[i].y * g_tentacle_ctrl.current.segments[i].y +
                               g_tentacle_ctrl.current.segments[i].z * g_tentacle_ctrl.current.segments[i].z);
        } else {
            float dx = g_tentacle_ctrl.current.segments[i].x - g_tentacle_ctrl.current.segments[i-1].x;
            float dy = g_tentacle_ctrl.current.segments[i].y - g_tentacle_ctrl.current.segments[i-1].y;
            float dz = g_tentacle_ctrl.current.segments[i].z - g_tentacle_ctrl.current.segments[i-1].z;
            total_length += sqrt(dx*dx + dy*dy + dz*dz);
        }
    }
    return total_length;
}

// 检查触角形状是否合理
uint8_t Tentacle_Check_Shape_Validity(void)
{
    // 检查各节是否按顺序排列
    for (int i = 1; i < TENTACLE_SEGMENTS; i++) {
        float prev_z = g_tentacle_ctrl.current.segments[i-1].z;
        float curr_z = g_tentacle_ctrl.current.segments[i].z;
        if (curr_z < prev_z) {
            DEBUG("Warning: Segment %d is below segment %d\n", i+1, i);
            return 0;
        }
    }
    
    // 检查半径是否递减
    for (int i = 1; i < TENTACLE_SEGMENTS; i++) {
        float prev_radius = g_tentacle_ctrl.current.segments[i-1].radius;
        float curr_radius = g_tentacle_ctrl.current.segments[i].radius;
        if (curr_radius > prev_radius) {
            DEBUG("Warning: Segment %d radius is larger than segment %d\n", i+1, i);
            return 0;
        }
    }
    
    return 1;
}
